#include <joyos.h>
#include <happylib.h>

// my headers
#include "define.h"
#include "goalscore.h"
#include "robomove.h"
#include "roboturn.h"
#include "led.h"
#include "bumper.h"
#include "distance.h"
#include "init.h"
#include "skunk.h"
#include "strategy.h"
#include "robotest.h"

void roboTest(void) {    
	startTest("Robotest v.0.001");
    go_click();
    
	startTest("isGoalLeftTest  ");
	isGoalRightTest();
	
	startTest("Wall Follow Left");
	wallFollowLeftTest();
	roboStop();
	
	startTest("Wall Follow Righ");
	wallFollowRightTest();
	roboStop();
	
	startTest("Score Test      ");
	scoreTest();


    //startTest("Init Test");
    //initTest();

	//startTest("Turn Test!");
	//printf_P (PSTR("\nCalibrate Gyro"));	
	//go_click ();
	//calibrateGyro();
	//turn(-90);
	//pause(1000);
	//turn(90);
	
	//startTest("Go Straight Test");
	//calibrateGyro();
	//clearEncoder();
	//roboStraightTest();
	//roboStop();
	
	
	startTest("isGoalLeftTest  ");
	isGoalLeftTest();
	
	startTest("isGoalLeftTest  ");
	isGoalRightTest();
		
	startTest("Turn Orient Test");
	toOrientTest();
	roboStop();

	startTest("Turn Test       ");
	roboTurnTest();
	roboStop();
	
	startTest("Wall Follow Left");
	wallFollowLeftTest();
	roboStop();
	
	startTest("Wall Follow Righ");
	wallFollowRightTest();
	roboStop();
	
	startTest("Score Test      ");
	scoreTest();

	startTest("Skunk Test      ");
	skunkTest();
	
	startTest("Distance Test   ");
	distanceTest();

	
	startTest("Initial Test    ");
	initTest();

	printf("\nTests complete.");
	while (1);
}

void startTest(char testName[]) {
	printf("\n%sPress Go",testName);
	go_click();
}

void roboStraightTest (void) {
	while (!stop_press()) {
		roboStraight(200);
        pause(50);
	}
}

void roboTurnTest (void) {
	printf_P (PSTR("\nCalibrate Gyro"));	
	go_click ();
	calibrateGyro();
	
	/*
	currentOrient = 1;
	printf_P(PSTR("currentOrient = 1"));
	pause(1000);
	turnToOrient(2);
	printf_P(PSTR("currentOrient = 2"));
	pause(1000);
	turnToOrient(0);
	printf_P(PSTR("currentOrient = 0"));
	pause(1000);
	turnToOrient(3);
	printf_P(PSTR("currentOrient = 3"));
	pause(1000);
	turnToOrient(1);
	printf_P(PSTR("currentOrient = 1"));
	pause(1000);
	*/

	turnWithShaft(90);
	pause(1000);
	turnWithShaft(-90);
	pause(1000);
}

void wallFollowLeftTest (void) {
	while (!stop_press()) {
		wallFollow(1, 150);
	}
}

void wallFollowRightTest (void) {
	while (!stop_press()) {
		wallFollow(0, 150);
	}
}

void scoreTest (void) {
	calibrateGoalServo();
	scoreLeft(2);
	scoreRight(2);
	scoreLeft(1);
	scoreRight(1);
}

void skunkTest (void) {
	calibrateSkunkServo();
	while (!stop_press()) {
		openSkunk();
		pause(750);
		closeSkunk();
		pause(750);
	}
}

void distanceTest (void) {
	while (!stop_press()) {
		printf_P(PSTR("\nDistance: %.2f"), getDistance());
		pause(50);
	}
}

void isGoalLeftTest (void) {
	caliLeftLed();
	while (!stop_press()) {
		printf_P(PSTR("\nisGoalOnTheLeft: %d"), isGoal(1));
		pause(50);
	}
}

void isGoalRightTest (void) {
	caliRightLed();
	while (!stop_press()) {
		printf("\nisGoalOnTheRight: %d", isGoal(0));
		pause(50);
	}
}


void initTest (void) {
	//caliUnderLed();
	while (!stop_press()) {
		go_click();
		initPose();
		printf_P (PSTR("\nPlace %d, orient %d"), initPlace, initOrient);
		pause(2000);
		initTurn();
		//go_click();
		
		//printf ("\nFR: %d FL: %d BR: %d PL: %d OR: %d", isBlack(LED_UNDER_FRONTRIGHT), isBlack(LED_UNDER_FRONTLEFT), isBlack(LED_UNDER_BACKRIGHT), initPlace, initOrient);
	}
}

void toOrientTest (void){
	currentOrient = NORTH;
	printf_P(PSTR("\nTurn from 0 to 1"));
	turnToOrient(EAST);
	pause(1000);
	printf_P(PSTR("\nTurn from 1 to 2"));
	turnToOrient(SOUTH);
	pause(1000);
	printf_P(PSTR("\nTurn from 2 to 1"));
	turnToOrient(EAST);
	pause(1000);
	printf_P(PSTR("\nTurn from 1 to 3"));
	turnToOrient(WEST);
	printf_P(PSTR("\nDONE"));
	
	go_click();
}
